#Controller Code

import motor
from numpy import *

def control(state,dt,ztarg,l):

	x = state[0]
	y = state[1]
	z = state[2]
	u = state[3]
	v = state[4]
	w = state[5]
	phi = state[6]
	theta = state[7]
	psi = state[8]
	p = state[9]
	q = state[10]
	r = state[11]
	
	err=z-ztarg
	
	#Gain
	Kp=-2
	Kd=1
	
	#altitude control
	fwdmot = 1+err*Kp+abs(w)*err/abs(err)*Kd
	rmot   = 1+err*Kp+abs(w)*err/abs(err)*Kd
	aftmot = 1+err*Kp+abs(w)*err/abs(err)*Kd
	lmot   = 1+err*Kp+abs(w)*err/abs(err)*Kd
	
	delta=array([fwdmot, rmot, aftmot, lmot])
	Force=motor.mot(delta,l)
	
	return Force